﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class InitModel : MonoBehaviour
{
        /// <summary>
        /// 模拟场地的ID
        /// </summary>
        public int areaId { get; set; }
        /// <summary>
        /// 锚点显示对象
        /// </summary>
        public Anchor[] anchors;
        /// <summary>
        /// 目标显示对象
        /// </summary>
        public TagLocal[] tags;
        /// <summary>
        /// 结果显示对象
        /// </summary>
        public Result[] results;
        [Header("描述文本")]
        public TextMesh description;
        [Header("失败描述文本")]
        public TextMesh fdes;
        /// <summary>
        /// 准备解算的模型
        /// </summary>
        BallInfo model;
        /// <summary>
        /// 解算结果1
        /// </summary>
        Vec3dInfo res1;
        /// <summary>
        /// 解算结果2
        /// </summary>
        Vec3dInfo res2;
        /// <summary>
        /// 最佳解算结果
        /// </summary>
        Vec3dInfo resBest;
        /// <summary>
        /// 解算失败的次数
        /// </summary>
        int tims = 0;
        /// <summary>
        /// 误差大于0.5的次数
        /// </summary>
        int errtims = 0;
        /// <summary>
        /// 是否开始模拟一次
        /// </summary>
        public bool isCreat = true;
        /// <summary>
        /// 总模拟次数
        /// </summary>
        long allTimes = 0;
        /// <summary>
        /// 出错是否自动暂停
        /// </summary>
        public static bool isAutoPuse = true;

        void Awake()
        {
                anchors = this.GetComponentsInChildren<Anchor>();
                tags = this.GetComponentsInChildren<TagLocal>();
                results = this.GetComponentsInChildren<Result>();

                if(anchors.Length < 3)
                {
                        Debug.LogError("锚点数量至少3个！");
                }

                if(tags.Length < 1)
                {
                        Debug.LogError("目标点数量至少1个！");
                }

                if(results.Length < 1)
                {
                        Debug.LogError("解数量至少1个！");
                }

                for (int i = 0; i < 3; i++)
                {
                        if (anchors[i] != null)
                        {
                                anchors[i].area = this.areaId;
                                anchors[i].id = i;
                                Debug.Log("锚点："+anchors[i].name);
                        }
                }

                if (tags[0] != null)
                {
                        tags[0].id = 0;
                        Debug.Log("目标点：" + tags[0].name);
                }

                if (results[0] != null)
                {
                        results[0].id = 0;
                        Debug.Log("解：" + results[0].name);
                }
        }


        void Update()
        {
                if(isCreat)
                {
                        isCreat = false;
                        int[] ra = new int[4];
                        Vec3dInfo[] ancArray = new Vec3dInfo[4];
                        res1 = new Vec3dInfo();
                        res2 = new Vec3dInfo();

                        //获取一个模型数据
                        model = RandomCreate.get_model();

                        Debug.Log(model.ToString());

                        for (int i = 0; i < 3; i++)
                        {
                                //设置锚点坐标
                                if (anchors[i] != null)
                                {
                                        anchors[i].SetAnchor(model.ball[i], model.r[i]);
                                }

                                //写入半径
                                ra[i] = (int)(model.r[i] * 1000);
                                //写入坐标
                                ancArray[i] = model.ball[i];
                        }

                        //设置目标点坐标
                        tags[0].SetTag(model.tag);

                       //计算
                        string des;
                        int r = AlgorithmTrilateration.GetLocation(ref resBest,ref res1, ref res2, 3, ref ancArray, ref ra);
                        allTimes++;
                        if (r == AlgorithmTrilateration.TRIL_3SPHERES)
                        {
                                //解算成功
                                results[0].SetResult(res1,0);
                                if (results.Length > 1)
                                {
                                        if (results[1] != null)
                                        {
                                                results[1].SetResult(res2,1);
                                        }
                                }
                                Debug.Log("解：(" + resBest.x.ToString("0.00") + "," + resBest.y.ToString("0.00") + "," + resBest.z.ToString("0.00") + ")");
                                
                                //误差太大显示红色
                                string color1 = "<color=red>";
                                string color2 = "<color=red>";
                                string colora = "<color=red>";
                                bool isErrorBig = true;
                                //任意一个解的误差小总的显示蓝色
                                if (GetDistance(res1, model.tag) < 0.5)
                                {
                                        color1 = "<color=blue>";
                                        colora = color1;
                                        isErrorBig = false;
                                }
                                if (GetDistance(res2,model.tag) < 0.5)
                                {
                                        color2 = "<color=blue>";
                                        colora = color2;
                                        isErrorBig = false;
                                }

                                if (isErrorBig)
                                {
                                        errtims++;
                                        //出错自动暂停
                                        if (isAutoPuse)
                                        {
                                                SettingMain.isAutoNextStopFlag = true;
                                        }
                                }

                                des = "1X差：" + color1 + (res1.x - model.tag.x).ToString("0.00") + "</color>" +
                                                "\t2X差：" + color2 + (res2.x - model.tag.x).ToString("0.00") + "</color>\r\n" +
                                                "1Y差：" + color1 + (res1.y - model.tag.y).ToString("0.00") + "</color>" +
                                                "\t2Y差：" + color2 + (res2.y - model.tag.y).ToString("0.00") + "</color>\r\n" +
                                                "1Z差：" + color1 + (res1.z - model.tag.z).ToString("0.00") + "</color>" +
                                                "\t2Z差：" + color2 + (res2.z - model.tag.z).ToString("0.00") + "</color>\r\n" +
                                                "最佳解总差：" + colora + Math.Pow(Math.Pow(resBest.x - model.tag.x, 2) + Math.Pow(resBest.y - model.tag.y, 2) + Math.Pow(resBest.z - model.tag.z, 2), 0.5).ToString("0.00") + "</color>\r\n"
                                                 ;
                                
                                description.text = des;
                        }
                        else
                        {
                                //解算失败
                                tims++;
                                Debug.LogWarning("第" + tims + "次解算失败！");
                                //出错自动暂停
                                if (isAutoPuse)
                                {
                                        SettingMain.isFailed = true;
                                }
                        }

                        des = "<color=red>模拟</color>" + allTimes + "<color=red>次</color>\r\n" +
                                "<color=red>失败</color>" + tims + "<color=red>次</color>\r\n" +
                                "<color=red>误差</color>" + errtims + "<color=red>次</color>\r\n";
                        fdes.text = des;
                }
        }

        double GetDistance(Vec3dInfo v1, Vec3dInfo v2)
        {
                double dis = Math.Pow(Math.Pow(v1.x - v2.x, 2) + Math.Pow(v1.y - v2.y, 2) + Math.Pow(v1.z - v2.z, 2), 0.5);
                return dis;
        }
}
